/*
This example takes range measurements with the VL53L1X and displays additional 
details (status and signal/ambient rates) for each measurement, which can help
you determine whether the sensor is operating normally and the reported range is
valid. The range is in units of mm, and the rates are in units of MCPS (mega 
counts per second).
*/

#include <Wire.h>
#include <VL53L1X.h>

VL53L1X sensor;
int LED=1;
void setup()
{
  //set LED as output device
  pinMode(LED, OUTPUT);
  //setup serial
  Serial.begin(115200);
  //setup I2C
  Wire.begin();
  Wire.setClock(400000); // use 400 kHz I2C

  //setup VL53L1X
  sensor.setTimeout(500);
  sensor.init();
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    while (1);
  }
  // Use long distance mode and allow up to 50000 us (50 ms) for a measurement.
  // You can change these settings to adjust the performance of the sensor, but
  // the minimum timing budget is 20 ms for short distance mode and 33 ms for
  // medium and long distance modes. See the VL53L1X datasheet for more
  // information on range and timing limits.
  sensor.setDistanceMode(VL53L1X::Long);
  sensor.setMeasurementTimingBudget(140000);

  // Start continuous readings at a rate of one measurement every 50 ms (the
  // inter-measurement period). This period should be at least as long as the
  // timing budget.
  sensor.startContinuous(140);

  //turn on LED to confirm sensor setup
  digitalWrite(LED,HIGH);
}

void loop()
{
  //read VL53L1X
  sensor.read();

  //send data: time, distance, status, peak, ambient
  Serial.print(millis()); //time
  
  Serial.print(",");  //splitting identifier
  Serial.print(sensor.ranging_data.range_mm);   //distance (mm)
  
  Serial.print(",");  //splitting identifier
  Serial.print(VL53L1X::rangeStatusToString(sensor.ranging_data.range_status));   //range status
  
  Serial.print(",");  //splitting identifier
  Serial.print(sensor.ranging_data.peak_signal_count_rate_MCPS);  //peak signal
  
  Serial.print(",");  //splitting identifier
  Serial.println(sensor.ranging_data.ambient_count_rate_MCPS);  //ambient signal
  //note last sent data uses println because we will use the \n to detect end of transmission of single data set
}

  // send framing
//  Serial.write(1);
//  Serial.write(2);
//  Serial.write(3);
//  Serial.write(4);

  //send reading
//  Serial.write(sensor.ranging_data.range_status);
//  Serial.write(sensor.ranging_data.range_mm & 255);
//  Serial.write((sensor.ranging_data.range_mm >> 8) & 255);

  // from VL53L1X datasheet
  // *RangeStatus: this 8 bit integer gives the range status for the current measurement. A
  //    value of 0 means the ranging is valid
  // *uint16_t range_mm
  //    Range reading from the last measurement, in millimeters. (This reading can also be obtained as the return value of read().)
  // *float peak_signal_count_rate_MCPS
  //    Peak signal count rate of the last measurement, in units of mega counts per second.
  // *float ambient_count_rate_MCPS
  //    Ambient count rate of the last measurement, in units of mega counts per second.

 //Serial.println(millis() + "," + sensor.ranging_data.range_mm + "," + sensor.ranging_data.range_status + "," + sensor.ranging_data.peak_signal_count_rate_MCPS + "," + sensor.ranging_data.ambient_count_rate_MCPS);